#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <core/balance_task.hpp>
#include <core/core.hpp>
#include <core/follow_path_task.hpp>
#include <core/idle_task.hpp>
#include <core/state_servo_task.hpp>
Core::Core() : Node("Core")
{
    RCLCPP_INFO(this->get_logger(), "Core node started");

    RCLCPP_INFO(this->get_logger(), "Create lifecycle server");
    switch_mode_server = rclcpp_action::create_server<SWMode>(
        this, "switch_mode",
        std::bind(&Core::OnGoal, this, std::placeholders::_1,
                  std::placeholders::_2),
        std::bind(&Core::OnCancel, this, std::placeholders::_1),
        std::bind(&Core::OnAccepted, this, std::placeholders::_1));
}

void Core::Initialize()
{
    declare_parameter<std::string>("prefix", "");
    std::string prefix = get_parameter("prefix").as_string();
    RCLCPP_INFO(this->get_logger(), "Create context");
    context = std::make_shared<Context>(weak_from_this(), prefix);
    RCLCPP_INFO(this->get_logger(), "Create tasks");
    tasks.insert({WorkMode::IDLE, std::make_shared<IdelTask>(context)});
    tasks.insert(
        {WorkMode::PATHFOLLOW, std::make_shared<FollowPathTask>(context)});
    tasks.insert(
        {WorkMode::STATESERVO, std::make_shared<StateServoTask>(context)});
    tasks.insert({WorkMode::BALANCE, std::make_shared<BalanceTask>(context)});

    current_task = tasks[WorkMode::BALANCE];
    current_task->OnInit();
}
rclcpp_action::GoalResponse Core::OnGoal(
    const rclcpp_action::GoalUUID &uuid,
    std::shared_ptr<const SWMode::Goal> goal)
{
    (void)uuid;
    (void)goal;
    if (dynamic_cast<IdelTask *>(current_task.get()) ||
        dynamic_cast<BalanceTask *>(current_task.get()))
    {
        if (tasks.find(goal.get()->mode.mode) == tasks.end())
        {
            RCLCPP_WARN(get_logger(), "Invalid goal, id: %d",
                        goal.get()->mode.mode);
            return rclcpp_action::GoalResponse::REJECT;
        }
        auto nextTask = tasks[goal.get()->mode.mode];
        if (nextTask == current_task)
        {
            RCLCPP_INFO(get_logger(), "Same task, ignore");
            return rclcpp_action::GoalResponse::REJECT;
        }
        RCLCPP_INFO(get_logger(), "Task try change to [%s]",
                    nextTask->getName().c_str());
        if (nextTask->OnInit())
        {
            current_task = nextTask;
            RCLCPP_INFO(get_logger(), "Task change to [%s]",
                        current_task->getName().c_str());
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
        }
        RCLCPP_WARN(get_logger(), "Task Reject for task init failed");
        return rclcpp_action::GoalResponse::REJECT;
    }
    // for emergency stop
    if (goal.get()->mode.mode == WorkMode::IDLE)
    {
        RCLCPP_INFO(get_logger(), "Emergency stop");
        current_task = tasks[WorkMode::IDLE];
        current_task->OnInit();
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    RCLCPP_INFO(get_logger(), "REJECT, ct:[%s],tt[%d]",
                current_task->getName().c_str(), goal.get()->mode.mode);
    return rclcpp_action::GoalResponse::REJECT;
}

rclcpp_action::CancelResponse Core::OnCancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<SWMode>> handle)
{
    (void)handle;
    RCLCPP_INFO(get_logger(), "Cancel request");
    if (current_task->OnCancle())
    {
        current_task = tasks[WorkMode::IDLE];
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    return rclcpp_action::CancelResponse::REJECT;
}

void Core::OnAccepted(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<SWMode>> handle)
{
    RCLCPP_INFO(get_logger(), "accepted request");
    SWMode::Feedback fb;
    int ret = current_task->OnStep(fb.precent);
    if (ret == 0)
    {
        handle->publish_feedback(std::make_shared<SWMode::Feedback>(fb));
    }
    else if (ret == 1)
    {
        SWMode::Result::SharedPtr result = std::make_shared<SWMode::Result>();
        result->success = true;
        handle->succeed(result);
    }
    else
    {
        SWMode::Result::SharedPtr result = std::make_shared<SWMode::Result>();
        result->success = false;
        handle->abort(result);
    }
}
